
#include "Integrator.h"
#include <math.h>
#include <stdio.h>
#include <malloc.h>

Integrator::Integrator(void)
{
}

Integrator::~Integrator(void)
{
}

void Integrator::InitIntegrator( int scale )
{
	this->scale = scale;

	int space = scale * 3;

	check_A = (float**)malloc(space*sizeof(float*));
	check_b = (float*)malloc(space*sizeof(float));
	check_buffer = (float*)malloc(space*sizeof(float));
	check_x = (float*)malloc(space*sizeof(float));
	check_residual = (float*)malloc(space*sizeof(float));
	check_p = (float*)malloc(space*sizeof(float));
	check_residual_cache = (float*)malloc(space*sizeof(float));

	for (int i=0;i<space;i++)
	{
		check_A[i] = (float*)malloc(space*sizeof(float));
		check_b[i] = 0;
		check_buffer[i] = 0;
		check_x[i] = 0;
		check_residual[i] = 0;
		check_p[i] = 0;
		check_residual_cache[i] = 0;

		for (int j=0;j<space;j++)
		{
			check_A[i][j] = 0;
		}
	}

}

void Integrator::SparseSolver()
{
	//CG();
	Jacobi();
}

void Integrator::CG()
{
	int scale = this->scale*3;

	for (int i=0;i<scale;i++)
	{
		double sum=0;

		for (int j=0;j<scale;j++)
		{
			sum+=check_A[i][j]*check_x[j];
		}

		check_residual[i]=check_b[i]-sum;
		check_p[i]=check_residual[i];
	}

	int loop;

	for (loop=0;loop<1000;loop++)
	{
		double alpha, sumU=0, sumD=0;

		for (int i=0;i<scale;i++)
		{
			sumU+=check_residual[i]*check_residual[i];
			double sumT=0;

			for (int j=0;j<scale;j++)
			{
				sumT+=check_p[j]*check_A[j][i];
			}

			sumD+=sumT*check_p[i];
		}

		alpha=sumU/sumD;

		for (int i=0;i<scale;i++)
		{
			check_buffer[i]=check_x[i]+alpha*check_p[i];

			double sum=0;

			for (int j=0;j<scale;j++)
			{
				sum+=check_A[i][j]*check_p[j];
			}

			check_residual_cache[i]=check_residual[i]-alpha*sum;
		}

		/************************************************************************/
		/* check here                                                                     */
		/************************************************************************/

		double r=abs(check_residual_cache[0]);

		for (int i=0;i<scale;i++)
		{
			if (r<abs(check_residual_cache[i]))
			{
				r=abs(check_residual_cache[i]);
			}
		}

		if (r<TOLERATE)
		{
			/*double *swap;
			swap=output;
			output=cache;
			cache=swap;*/
			float swap;

			for (int m=0;m<scale;m++)
			{
				swap = check_x[m];
				check_x[m] = check_buffer[m];
				check_buffer[m] = swap;
			}

			printf("%d\n",loop);

			return;
		}

		double beta;
		sumU=0;
		sumD=0;

		for (int i=0;i<scale;i++)
		{
			sumU+=check_residual_cache[i]*check_residual_cache[i];
			sumD+=check_residual[i]*check_residual[i];
		}

		beta=sumU/sumD;

		for (int i=0;i<scale;i++)
		{
			check_p[i]=check_residual_cache[i]+beta*check_p[i];
		}

		/*double *swap;
		swap=residual;
		residual=residual_cache;
		residual_cache=swap;

		swap=output;
		output=cache;
		cache=swap;*/

		float swap;

		for (int m=0;m<scale;m++)
		{
			swap = check_residual[m];
			check_residual[m] = check_residual_cache[m];
			check_residual_cache[m] = swap;

			swap = check_x[m];
			check_x[m] = check_buffer[m];
			check_buffer[m] = swap;
		}
	}

	printf("%d\n",loop);
}

void Integrator::Jacobi()
{
	int scale = this->scale*3;

	for(int loop=0;loop<1000;loop++)
	{
		for (int i=0;i<scale;i++)
		{
			float sum=0;

			for (int j=0;j<scale;j++)
			{
				if (i!=j)
				{
					sum+=check_A[i][j] * check_x[j];
				}
			}

			if (check_A[i][i]!=0)
			{
				check_buffer[i] = (check_b[i] - sum) / check_A[i][i];
			}else{
				printf("wrong\n");
			}
		}

		float residualMax=0;

		for (int i=0;i<scale;i++)
		{
			if (residualMax<abs(check_buffer[i]-check_x[i]))
			{
				residualMax=abs(check_buffer[i]-check_x[i]);
			}
			check_x[i]=check_buffer[i];
		}

		if (residualMax<TOLERATE)
		{
			printf("@%d",loop);
			break;
		}
	}
}